﻿using OpenCvSharp;
using System.Drawing;
using VIA.Integration.Domain;

namespace VIA.SiMoJi.Domain.Vision
{
    public static class CMatrixEx
    {
        public static Pose ToPose(this CMatrix matrix)
        {
            double cx = matrix.M11;
            double sx = matrix.M12;
            double angle = 0;
            angle = Math.Asin(sx) * 180.0 / Math.PI;
            if (angle > 0 && cx < 0) angle = 180.0 - angle;
            if (angle <= 0 && cx < 0) angle = -180.0 - angle;
            return new Pose(matrix.OffsetX, matrix.OffsetY, angle);
        }

        public static VIAPoint Transform(this CMatrix matrix, VIAPoint point)
        {
            PointF pt = new((float)point.X, (float)point.Y);
            PointF pt1 = matrix.Transform(pt);
            return new VIAPoint(pt1.X, pt1.Y);
        }

        public static Pose Transform(this CMatrix matrix, Pose pose)
        {
            var mat = pose.ToMatrix();
            var newMat = mat * matrix;
            var newPose = newMat.ToPose();
            return newPose;
        }

        public static PointF? GetIntersectionPoint(PointF p1, PointF p2, PointF p3, PointF p4)
        {
            float denom = (p4.X - p3.X) * (p2.Y - p1.Y) - (p2.X - p1.X) * (p4.Y - p3.Y);
            if (denom == 0) return null;
            float t1 = ((p3.X - p1.X) * (p3.Y - p4.Y) - (p3.Y - p1.Y) * (p3.X - p4.X)) / denom;
            return new PointF(p1.X + t1 * (p2.X - p1.X), p1.Y + t1 * (p2.Y - p1.Y));
        }

        public static VIAPoint? GetIntersectionPoint(VIAPoint p1, VIAPoint p2, VIAPoint p3, VIAPoint p4)
        {
            double denom = (p4.X - p3.X) * (p2.Y - p1.Y) - (p2.X - p1.X) * (p4.Y - p3.Y);
            if (denom == 0) return null;
            double t1 = ((p3.X - p1.X) * (p3.Y - p4.Y) - (p3.Y - p1.Y) * (p3.X - p4.X)) / denom;
            return new VIAPoint(p1.X + t1 * (p2.X - p1.X), p1.Y + t1 * (p2.Y - p1.Y));
        }

        public static VIAPoint ToVIAPoint(this Pose pose)
        {
            return new VIAPoint(pose.X, pose.Y);
        }

        public static CMatrix CalcMatrix(VIAPoint[] originalPoints, VIAPoint[] destPoints)
        {
            var length = destPoints.Count();
            if (length != originalPoints.Count())
                throw new ArgumentException("机器人坐标点个数必须和图像坐标点个数一致");
            Mat from = new Mat(length, 2, MatType.CV_64F);
            Mat to = new Mat(length, 2, MatType.CV_64F);
            for (int i = 0; i < length; i++)
            {
                var p1 = originalPoints[i];
                var p2 = destPoints[i];

                from.Set<double>(i, 0, p1.X);
                from.Set<double>(i, 1, p1.Y);

                to.Set<double>(i, 0, p2.X);
                to.Set<double>(i, 1, p2.Y);

            }
            var dst = Cv2.EstimateAffine2D(from, to);
            if (dst == null || dst.Cols == 0 || dst.Rows == 0)
                throw new Exception("标定矩阵生成失败");
            var matrix = new CMatrix(dst.Get<double>(0, 0), dst.Get<double>(1, 0), dst.Get<double>(0, 1), dst.Get<double>(1, 1), dst.Get<double>(0, 2), dst.Get<double>(1, 2));
            from.Dispose();
            to.Dispose();
            dst.Dispose();
            return matrix;
        }

        public static CMatrix CalcMatrix(Pose[] originalPose, Pose[] destPose, out double rmse)
        {
            var length = destPose.Count();
            if (length != originalPose.Count())
                throw new ArgumentException("机器人坐标点个数必须和图像坐标点个数一致");
            Mat from = new Mat(length, 2, MatType.CV_64F);
            Mat to = new Mat(length, 2, MatType.CV_64F);
            for (int i = 0; i < length; i++)
            {
                var p1 = originalPose[i];
                var p2 = destPose[i];

                from.Set<double>(i, 0, p1.X);
                from.Set<double>(i, 1, p1.Y);

                to.Set<double>(i, 0, p2.X);
                to.Set<double>(i, 1, p2.Y);
            }
            var dst = Cv2.EstimateAffine2D(from, to);
            if (dst == null || dst.Cols == 0 || dst.Rows == 0)
                throw new Exception("标定矩阵生成失败");
            var matrix = new CMatrix(dst.Get<double>(0, 0), dst.Get<double>(1, 0), dst.Get<double>(0, 1), dst.Get<double>(1, 1), dst.Get<double>(0, 2), dst.Get<double>(1, 2));
            from.Dispose();
            to.Dispose();
            dst.Dispose();
            rmse = RMSE(matrix, originalPose, destPose);
            return matrix;
        }

        public static double RMSE(CMatrix matrix, Pose[] originalPose, Pose[] destPose)
        {
            int ct = originalPose.Length;
            double sumXY = 0;
            for (int i = 0; i < ct; i++)
            {
                Pose delt = matrix.Transform(originalPose[i]) - destPose[i];
                var deltX = delt.X * delt.X;
                var deltY = delt.Y * delt.Y;
                sumXY += deltX + deltY;
            }
            double rmse = Math.Sqrt(sumXY / ct);
            return rmse;
        }

        public static bool CaculateCenterAndR(double x1, double y1, double x2, double y2, double x3, double y3, out double cx, out double cy, out double radius)
        {
            cx = 0; cy = 0; radius = 0;
            double sx1 = x1 * x1;
            double sy1 = y1 * y1;
            double sx2 = x2 * x2;
            double sy2 = y2 * y2;
            double sx3 = x3 * x3;
            double sy3 = y3 * y3;
            double A = x1 * (y2 - y3) - y1 * (x2 - x3) + x2 * y3 - x3 * y2;
            if (A == 0) return false;
            double B = (sx1 + sy1) * (y3 - y2) + (sx2 + sy2) * (y1 - y3) + (sx3 + sy3) * (y2 - y1);
            double C = (sx1 + sy1) * (x2 - x3) + (sx2 + sy2) * (x3 - x1) + (sx3 + sy3) * (x1 - x2);
            double D = (sx1 + sy1) * (x3 * y2 - x2 * y3) + (sx2 + sy2) * (x1 * y3 - x3 * y1) + (sx3 + sy3) * (x2 * y1 - x1 * y2);
            cx = -B / (A * 2);
            cy = -C / (A * 2);
            radius = Math.Sqrt((B * B + C * C - 4 * A * D) / (4 * A * A));
            return true;
        }

        public static void FitEllipes(VIAPoint[] points, out double cx, out double cy, out double radius)
        {
            var count = points.Count();
            Mat mat = new Mat(count, 2, MatType.CV_64F);
            for (int i = 0; i < count; i++)
            {
                mat.Set(i, 0, points[i].X);
                mat.Set(i, 1, points[i].Y);
            }
            var rotatedRect = Cv2.FitEllipse(mat);
            cx = rotatedRect.Center.X;
            cy = rotatedRect.Center.Y;
            radius = (rotatedRect.Size.Width + rotatedRect.Size.Height) / 2;
        }
    }
}
